#include "matrix4d.h"



Matrix4d&
Matrix4d::operator=(const Matrix4d& m)
{
	for (int i = 0; i < 4; ++i)
	{
		for (int j = 0; j < 4; ++j)
		{
			_m[i][j] = m._m[i][j];
		}
	}

	return *this;
}


Matrix4d&
Matrix4d::Inverse()
{
	DBL d00, d01, d02, d03;
	DBL d10, d11, d12, d13;
	DBL d20, d21, d22, d23;
	DBL d30, d31, d32, d33;
	DBL m00, m01, m02, m03;
	DBL m10, m11, m12, m13;
	DBL m20, m21, m22, m23;
	DBL m30, m31, m32, m33;
	DBL D;

	m00 = _m[0][0];  m01 = _m[0][1];  m02 = _m[0][2];  m03 = _m[0][3];
	m10 = _m[1][0];  m11 = _m[1][1];  m12 = _m[1][2];  m13 = _m[1][3];
	m20 = _m[2][0];  m21 = _m[2][1];  m22 = _m[2][2];  m23 = _m[2][3];
	m30 = _m[3][0];  m31 = _m[3][1];  m32 = _m[3][2];  m33 = _m[3][3];

	d00 = m11*m22*m33 + m12*m23*m31 + m13*m21*m32 - m31*m22*m13 - m32*m23*m11 - m33*m21*m12;
	d01 = m10*m22*m33 + m12*m23*m30 + m13*m20*m32 - m30*m22*m13 - m32*m23*m10 - m33*m20*m12;
	d02 = m10*m21*m33 + m11*m23*m30 + m13*m20*m31 - m30*m21*m13 - m31*m23*m10 - m33*m20*m11;
	d03 = m10*m21*m32 + m11*m22*m30 + m12*m20*m31 - m30*m21*m12 - m31*m22*m10 - m32*m20*m11;

	d10 = m01*m22*m33 + m02*m23*m31 + m03*m21*m32 - m31*m22*m03 - m32*m23*m01 - m33*m21*m02;
	d11 = m00*m22*m33 + m02*m23*m30 + m03*m20*m32 - m30*m22*m03 - m32*m23*m00 - m33*m20*m02;
	d12 = m00*m21*m33 + m01*m23*m30 + m03*m20*m31 - m30*m21*m03 - m31*m23*m00 - m33*m20*m01;
	d13 = m00*m21*m32 + m01*m22*m30 + m02*m20*m31 - m30*m21*m02 - m31*m22*m00 - m32*m20*m01;

	d20 = m01*m12*m33 + m02*m13*m31 + m03*m11*m32 - m31*m12*m03 - m32*m13*m01 - m33*m11*m02;
	d21 = m00*m12*m33 + m02*m13*m30 + m03*m10*m32 - m30*m12*m03 - m32*m13*m00 - m33*m10*m02;
	d22 = m00*m11*m33 + m01*m13*m30 + m03*m10*m31 - m30*m11*m03 - m31*m13*m00 - m33*m10*m01;
	d23 = m00*m11*m32 + m01*m12*m30 + m02*m10*m31 - m30*m11*m02 - m31*m12*m00 - m32*m10*m01;

	d30 = m01*m12*m23 + m02*m13*m21 + m03*m11*m22 - m21*m12*m03 - m22*m13*m01 - m23*m11*m02;
	d31 = m00*m12*m23 + m02*m13*m20 + m03*m10*m22 - m20*m12*m03 - m22*m13*m00 - m23*m10*m02;
	d32 = m00*m11*m23 + m01*m13*m20 + m03*m10*m21 - m20*m11*m03 - m21*m13*m00 - m23*m10*m01;
	d33 = m00*m11*m22 + m01*m12*m20 + m02*m10*m21 - m20*m11*m02 - m21*m12*m00 - m22*m10*m01;

	D = m00*d00 - m01*d01 + m02*d02 - m03*d03;

	if (D == 0.0)
	{
		_invalid = true;
	}

	_m[0][0] =  d00/D; _m[0][1] = -d10/D;  _m[0][2] =  d20/D; _m[0][3] = -d30/D;
	_m[1][0] = -d01/D; _m[1][1] =  d11/D;  _m[1][2] = -d21/D; _m[1][3] =  d31/D;
	_m[2][0] =  d02/D; _m[2][1] = -d12/D;  _m[2][2] =  d22/D; _m[2][3] = -d32/D;
	_m[3][0] = -d03/D; _m[3][1] =  d13/D;  _m[3][2] = -d23/D; _m[3][3] =  d33/D;
}


Matrix4d&
Matrix4d::Transpose()
{
	double n = 0;

	for (unsigned int i = 0 ; i < 4; ++i)
	{
		for (unsigned int j = i; j < 4; ++j)
		{
			n = _m[i][j];
			_m[i][j] = _m[j][i];
			_m[j][i] = n;
		}
	}

	return *this;
}


Matrix4d
Matrix4d::GetInverse()
{
	Matrix4d r = *this;

	r.Inverse();

	return r;
}


Matrix4d
Matrix4d::GetTranspose()
{
	Matrix4d r = *this;

	r.Transpose();
}


Matrix4d operator *(const Matrix4d& a, const Matrix4d& b)
{
	Matrix4d r;

	for (unsigned int i = 0; i < 4; ++i)
	{
		for (unsigned int j = 0; j < 4; ++j)
		{
			r[i][j] = a._m[i][0] * b._m[0][j] + a._m[i][1] * b._m[1][j] + a._m[i][2] * b._m[2][j] + a._m[i][3] * b._m[3][j];
		}
	}

	return r;
}


Matrix4d operator *(const Vector3d & v, const Mtarix4d& a)
{
	Vector3d r;

	r._x = a._m[0][0] * r._x + a._m[1][0] * r._y + a._m[2][0] * r._z + a._m[3][0];
	r._y = a._m[0][1] * r._x + a._m[1][1] * r._y + a._m[2][1] * r._z + a._m[3][1];
	r._z = a._m[0][2] * r._x + a._m[1][2] * r._y + a._m[2][2] * r._z + a._m[3][2];

	return r;
}


Matrix4d&
Matrix4d::Identity()
{
	for(unsigned int i = 0; i < 4; ++i)
	{
		for (unsigned int j = 0; j < 4; ++j)
		{
			_m[i][j] = (i == j) ? 1.0 : 0.0;
		}
	}

	return *this;
}


Matrix4d&
Matrix4d::Translate(double x, double y, double z)
{
	Matrix4d m(1.0);

	m[3][0] = x; m[3][1] = y; m[3][2] = z;

	return (*this *= m);
}


Matrix4d&
Matrix4d::Scale(double x, double y, double z)
{
	_m[0][0] *= x; _m[1][0] *= x; _m[2][0] *=x;
	_m[0][1] *= y; _m[1][1] *= y; _m[2][1] *=y;
	_m[0][2] *= z; _m[1][2] *= z; _m[2][2] *=z;

	return *this;
}


Matrix4d&
Matrix4d::RotateX(double n)
{
	double rad = fmod(n, 360.0);
	if (rad < 0) rad = 360 - rad;
	rad *= (PI / 180.0);

	Matrix4d m(1);
	double s = sin(rad), c = cos(rad);

	m[1][1] = c;
	m[2][1] = -s;
	m[1][2] = s;
	m[2][2] = c;

	return (*this *= m);
}


Matrix4d&
Matrix4d::RotateY(double n)
{
	double rad = fmod(n, 360.0);
	if (rad < 0) rad = 360 - rad;
	rad *= (PI / 180.0);

	Matrix4d m(1);
	double s = sin(rad), c = cos(rad);

	m[0][0] = c;
	m[2][0] = s;
	m[0][2] = -s;
	m[2][2] = c;

	return (*this *= m);
}


Matrix4d&
Matrix4d::RotateZ(double n)
{
	double rad = fmod(n, 360.0);
	if (rad < 0) rad = 360 - rad;
	rad *= (PI / 180.0);

	Matrix4d m(1);
	double s = sin(rad), c = cos(rad);

	m[0][0] = c;
	m[1][0] = -s;
	m[0][1] = s;
	m[1][1] = c;

	return (*this *= m);
}


Matrix4d&
Matrix4d::RotateAxis(double n, const Vector3d& v)
{
	double rad = fmod(n, 360.0);
	if (rad < 0) rad = 360 - rad;
	rad *= (PI / 180.0);

	double cosx = cos(rad), sinx = sin(rad);

	Identity();

	_m[0][0] = v.X() * v.X() + cosx * (1.0 - v.X() * v.X());
	_m[1][0] = v.X() * v.Y() * (1.0 - cosx) + v.Z() * sinx;
	_m[2][0] = v.X() * v.Z() * (1.0 - cosx) - v.Y() * sinx;

	_m[0][1] = v.X() * v.Y() * (1.0 - cosx) - v.Z() * sinx;
	_m[1][1] = v.Y() * v.Y() + cosx * (1.0 - v.Y() * v.Y());
	_m[2][1] = v.Y() * v.Z() * (1.0 - cosx) + v.X() * sinx;

	_m[0][2] = v.X() * v.Z() * (1.0 - cosx) + v.Y() * sinx;
	_m[1][2] = v.Y() * v.Z() * (1.0 - cosx) - v.X() * sinx;
	_m[2][2] = v.Z() * v.Z() + cosx * (1.0 - v.Z() * v.Z());

	return *this;
}






